#include <iostream>
#include <queue>
#include <vector>
#include "../Moduels/Windows/Windows.hpp"
#include "../Moduels/SerialPort/SerialPort.hpp"
#include "../Moduels/SerialPortUtilities/BytesAccessor.hpp"

int main() {

    std::vector<RoboPioneers::Lidar::UI::Widgets> widgets;
    std::deque<RoboPioneers::Lidar::UI::Widgets*> Q;
    auto last_time = std::chrono::steady_clock::now();

#ifdef REFEREE_SYSTEM
    auto SerialPort = RoboPioneers::SerialPort::Port("ttyUSB0");
    SerialPort.Open();
#endif

    int Mode = 0; // 0 第一个窗口为底 以此类推 0-2
    widgets.push_back(RoboPioneers::Lidar::UI::Widgets(1920,1080,2));
    widgets.push_back(RoboPioneers::Lidar::UI::Widgets(1920,1080,4));
    widgets.push_back(RoboPioneers::Lidar::UI::Widgets(1920,1080,6));

    Q.push_back(&widgets[0]);
    Q.push_back(&widgets[1]);
    Q.push_back(&widgets[2]);

    while (true)
    {
        RoboPioneers::Lidar::UI::Widgets *ptr;
        char time[16];
        cv::Mat window=cv::Mat::zeros(1080,1920,CV_8UC3);
        // 分配布局
        Q.at(0)->SetSize(1920,1080);
        Q.at(1)->SetSize(640,400);
        Q.at(2)->SetSize(640,400);

        // 获取图像
        widgets[0].GrabPicture();
        widgets[1].GrabPicture();
        widgets[2].GrabPicture();

        Q.at(0)->MixPicture(window,0,0);
        Q.at(1)->MixPicture(window,0,0);
        Q.at(2)->MixPicture(window,1280,0);

        auto current_time = std::chrono::steady_clock::now();
        auto duration = std::chrono::duration_cast<std::chrono::seconds>(current_time-last_time).count();
        duration = 6*60 - duration;
        int minute = duration / 60;
        int second = duration % 60;
        sprintf(time,"Time: %02d:%02d",minute,second);
        cv::putText(window,
                    std::string(time),
                    cv::Point(650,100),
                    cv::FONT_HERSHEY_TRIPLEX,
                    3,
                    cv::Scalar(255,0,0),
                    3);
        cv::imshow("Main",window);
        char key = cv::waitKey(1);
        // 更新对列
#ifdef KeyBoard

        if(key=='t')
        {
            ptr = Q.front(); Q.pop_front(); Q.push_back(ptr);
        }
#endif

#ifdef REFEREE_SYSTEM
       auto key =  SerialPort.ReadBytes();
        switch (key[0]) {
            case static_cast<unsigned char>(1):ptr = Q.front(); Q.pop_front(); Q.push_back(ptr); break;
            case static_cast<unsigned char>(2):ptr = Q.front(); Q.pop_front(); Q.push_back(ptr); break;
            case static_cast<unsigned char>(3):ptr = Q.front(); Q.pop_front(); Q.push_back(ptr); break;
            default: exit(1);
        }
#endif
    }

    return 0;
}
